// --------------------------------------------------------------------------------------------------------------------
// <copyright file="ExternalDatasetSensorFileReader.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Navigation
{
    using System;
    using System.Collections.Concurrent;
    using System.IO;
    using System.Threading;

    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.IO;
    using Microsoft.Robotics.Navigation.Perception;
    using Microsoft.Robotics.Navigation.Runtime.Perception;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Tracing;
    
    /// <summary>
    /// The file reader that can read the dataset files and can generate agent messages based on them
    /// </summary>
    public class ExternalDatasetSensorFileReader : IFileReader, IDisposable
    {
        /// <summary>
        /// Previous pose.
        /// </summary>
        private static Pose2D previousPose;

        /// <summary>
        /// The FileStream to open and write 
        /// </summary>
        private FileStream fileStream;

        /// <summary>
        /// The queue of deserialized messages waiting to be published at the right time
        /// </summary>
        private BlockingCollection<AgentMessage> publishingQueue;

        /// <summary>
        /// Indicates if the reader has more messages to publish.
        /// </summary>
        private CancellationTokenSource cancellationSource;

        /// <summary>
        /// Noise parameters.
        /// </summary>
        private PoseNoiseModelParams noiseParams;

        /// <summary>
        /// Initializes a new instance of the <see cref="ExternalDatasetSensorFileReader"/> class.
        /// </summary>
        /// <param name="noiseParams">Nose model parameters.</param>
        public ExternalDatasetSensorFileReader(PoseNoiseModelParams noiseParams)
        {
            this.noiseParams = noiseParams;
        }

        /// <summary>
        /// Parses a laser line and generates an HDP message from it.
        /// Ex: <![CDATA[FLASER 3 6.91 6.06 5.41 48.8103 pippo 48.8103]]>
        /// </summary>
        /// <param name="parts">The line to parse</param>
        /// <returns>The equivalent HDP message</returns>
        public static SimpleAgentMessage<LaserHorizontalDepthProfile> ParseHDPMessage(string[] parts)
        {
            int numberOfReadings = int.Parse(parts[1]);
            double timestamp = double.Parse(parts[parts.Length - 1]);
            long originatingTime = (long)(timestamp * 1000);
            double[] readings = new double[numberOfReadings];

            for (int i = 0; i < numberOfReadings; i++)
            {
                // Flip the readings from left to right to match our HDP
                readings[numberOfReadings - 1 - i] = double.Parse(parts[i + 2]);
            }

            LaserHorizontalDepthProfile hdp = new LaserHorizontalDepthProfile(readings, new Pose2D());
            SimpleAgentMessage<LaserHorizontalDepthProfile> hdpMessage = new SimpleAgentMessage<LaserHorizontalDepthProfile>(hdp, originatingTime);
            hdpMessage.Timestamp = originatingTime;
            return hdpMessage;
        }

        /// <summary>
        /// Parses an odometry line and generates an odometry message from it.
        /// Ex: <![CDATA[ODOM -0.96257 0.013485 1.67914 0 0 0 48.8103 pippo 48.8103]]>
        /// </summary>
        /// <param name="parts">The line to parse</param>
        /// <param name="lastMessage">The previous odometry message</param>
        /// <param name="noiseParams">Noise parameters</param>
        /// <returns>The parsed odometry message</returns>
        public static SimpleAgentMessage<PoseFeedbackState> ParseOdometryMessage(string[] parts, SimpleAgentMessage<PoseFeedbackState> lastMessage, PoseNoiseModelParams noiseParams)
        {
            PoseFeedbackState lastState = lastMessage.State;
            double x = double.Parse(parts[1]);
            double y = double.Parse(parts[2]);
            double heading = double.Parse(parts[3]);
            double timestamp = double.Parse(parts[parts.Length - 1]);
            long originatingTime = (long)(timestamp * 1000);
            
            PoseFeedbackState state = new PoseFeedbackState(new Pose2D(x, y, heading), noiseParams);
            Pose2D dfsPose = state.GetUpdatedPose(previousPose, lastState);

            SimpleAgentMessage<PoseFeedbackState> driveFeedbackMessage = new SimpleAgentMessage<PoseFeedbackState>(state, originatingTime);
            driveFeedbackMessage.Timestamp = originatingTime;
            previousPose = new Pose2D(x, y, heading);
            return driveFeedbackMessage;
        }

        /// <summary>
        /// Initializes a new instance of the ExternalDatasetSensorFileReader class
        /// </summary>
        /// <param name="filePath">The file path</param>
        /// <param name="publishingQueue">The queue where to store the messages deserialized so far</param>
        /// <returns>The recording start time, if persisted in the file</returns>
        public long Initialize(string filePath, BlockingCollection<AgentMessage> publishingQueue)
        {
            this.publishingQueue = publishingQueue;
            this.fileStream = new FileStream(filePath, FileMode.Open, FileAccess.Read);
            return 0;
        }

        /// <summary>
        /// Deserializes messages and inserts them in the publishing queue.
        /// This method blocks until Stop is called or until the end of file is reached.
        /// </summary>
        public void Start()
        {
            this.cancellationSource = new CancellationTokenSource();
            CancellationToken cancellationToken = this.cancellationSource.Token;

            PoseFeedbackState state = new PoseFeedbackState(new Pose2D(), this.noiseParams);
            SimpleAgentMessage<PoseFeedbackState> driveMessage = new SimpleAgentMessage<PoseFeedbackState>(state, AgentMessage.GetCurrentTime());
            using (this.fileStream)
            {
                // loop over all entries in the file and add them to the queue.
                // the queue will block if we are going too fast.
                StreamReader reader = new StreamReader(this.fileStream);
                try
                {
                    while (!reader.EndOfStream && !cancellationToken.IsCancellationRequested)
                    {
                        string line = reader.ReadLine();
                        if (string.IsNullOrEmpty(line))
                        {
                            continue;
                        }

                        string[] parts = line.Split();
                        if (parts[0] == "ODOM")
                        {
                            // build an odometry message
                            driveMessage = ParseOdometryMessage(parts, driveMessage, this.noiseParams);
                            this.publishingQueue.Add(driveMessage, cancellationToken);
                        }
                        else if (parts[0] == "FLASER")
                        {
                            // build a horizontal depth profile message
                            AgentMessage message = ParseHDPMessage(parts);
                            this.publishingQueue.Add(message, cancellationToken);
                        }
                        else
                        {
                            continue;
                        }
                    }
                }
                catch (OperationCanceledException)
                {
                    // do nothing, the agent cancelled the operation
                }

                this.publishingQueue.CompleteAdding();
            }
        }

        /// <summary>
        /// Stops listening for messages
        /// </summary>
        public void Stop()
        {
            this.cancellationSource.Cancel();
        }

        /// <summary>
        /// Disposes of an instance of the <see cref="ExternalDatasetSensorFileReader"/> class.
        /// </summary>
        public void Dispose()
        {
            this.Dispose(true);
        }

        /// <summary>
        /// Disposes of an instance of the <see cref="ExternalDatasetSensorFileReader"/> class.
        /// </summary>
        /// <param name="disposing">True if disposing, false if finalizing.</param>
        private void Dispose(bool disposing)
        {
            if (disposing)
            {
                if (this.cancellationSource != null)
                {
                    this.cancellationSource.Dispose();
                }

                if (this.fileStream != null)
                {
                    this.fileStream.Dispose();
                }

                GC.SuppressFinalize(this);
            }
        }
    }
}
